#include "mpu9250.h"

const uint8_t mpu_dummy_tx[24] = {
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff
};

uint8_t mpu_rx_buf[32] = {0};

uint8_t mpu9250_write_reg(MPU9250_HandleTypeDef* mpu9250,
			  uint8_t reg_addr,
			  uint8_t reg_data);
uint8_t mpu9250_read_reg(MPU9250_HandleTypeDef* mpu9250, uint8_t reg_addr);
void mpu9250_read_regs(MPU9250_HandleTypeDef* mpu9250,
		       uint8_t reg_addr,
		       uint8_t* data,
		       uint8_t len);
uint8_t mpu9250_nonblocking_init(MPU9250_HandleTypeDef* mpu);
void mpu9250_handle_all_byte(void* pMpu);
void mpu9250_handle_imu_byte(void* pMpu);

void mpu9250_nonblocking_process(MPU9250_HandleTypeDef* mpu)
{
	uint8_t tx_byte[32] = {0}; // arbitrary length
	uint8_t i;
	static uint8_t imu_read_ntimes = 0;

	switch (mpu->status) {
	case INIT:
		if (INIT_MAG_SEND_CTRL == mpu9250_nonblocking_init(mpu))
			mpu->status = CHECK_FS;
		return;
	case CHECK_FS:
		mpu9250_read_regs(mpu, MPUREG_GYRO_CONFIG, mpu->rx_byte, 2);
		mpu->initStructure.gyr_fs = mpu->rx_byte[0]
			& MPU9250_GFS_BITMASK;
		mpu->initStructure.acc_fs = mpu->rx_byte[1]
			& MPU9250_AFS_BITMASK;
		imu_read_ntimes = 0;
		mpu->status = UPDATE_MAG_SENS;
		return;
	case UPDATE_MAG_SENS:
		tx_byte[0] = MPUREG_I2C_SLV0_ADDR;
		tx_byte[1] = AK8963_I2C_ADDR | READ_FLAG;
		tx_byte[2] = AK8963_ASAX;
		tx_byte[3] = 0x83;
		spi_select(MPU9250_CS_PIN, 1);
		HAL_SPI_Transmit(mpu->spi, tx_byte, 4, 1000);
		spi_select(MPU9250_CS_PIN, 0);
		mpu->status = READ_MAG_SENS;
		return;
	case READ_MAG_SENS:
		mpu9250_read_regs(mpu, MPUREG_EXT_SENS_DATA_00,
				  mpu->rx_byte, 3);
		for (i = 0; i < 3; i++) {
			mpu->magSensAdj[i] = ((mpu->rx_byte[i] - 128)/256.0f + 1.0f);
		}
		mpu->status = READ_IMU;
		return;
	case READ_IMU:
		tx_byte[0] = MPUREG_ACCEL_XOUT_H | READ_FLAG;
		spi_select(MPU9250_CS_PIN, 1);
		HAL_SPI_Transmit(mpu->spi, tx_byte, 1, 1000);
		HAL_SPI_TransmitReceive_DMA(mpu->spi, mpu_dummy_tx,
					    mpu->rx_byte, 14);
		mpu->raw_data.imu.timestamp = board_get_systime();
		mpu->rx_handler = mpu9250_handle_imu_byte;
		if (imu_read_ntimes++ >= mpu->readMagAfterNImu) {
			imu_read_ntimes = 0;
			mpu->status = UPDATE_MAG_DATA;
		}
		return;
	case UPDATE_MAG_DATA:
		tx_byte[0] = MPUREG_I2C_SLV0_ADDR;
		tx_byte[1] = AK8963_I2C_ADDR | READ_FLAG;
		tx_byte[2] = AK8963_HXL;
		tx_byte[3] = 0x87;
		//			mpu9250_write_reg(mpu, MPUREG_I2C_SLV0_ADDR, AK8963_I2C_ADDR | READ_FLAG);
		//			mpu9250_write_reg(mpu, MPUREG_I2C_SLV0_REG, AK8963_HXL);
		//			mpu9250_write_reg(mpu, MPUREG_I2C_SLV0_CTRL, 0x87);
		spi_select(MPU9250_CS_PIN, 1);
		HAL_SPI_Transmit(mpu->spi, tx_byte, 4, 1000);
		spi_select(MPU9250_CS_PIN, 0);
		mpu->status = READ_ALL;
		return;
	case READ_ALL:
		tx_byte[0] = MPUREG_ACCEL_XOUT_H | READ_FLAG;
		spi_select(MPU9250_CS_PIN, 1);
		HAL_SPI_Transmit(mpu->spi, tx_byte, 1, 1000);
		HAL_SPI_TransmitReceive_DMA(mpu->spi, mpu_dummy_tx,
					    mpu->rx_byte, 21);
		mpu->raw_data.imu.timestamp = board_get_systime();
		mpu->raw_data.mag.timestamp = mpu->raw_data.imu.timestamp;
		mpu->rx_handler = mpu9250_handle_all_byte;
		mpu->status = READ_IMU;
		return;
	case SET_GFS:
		mpu->status = READ_IMU;
	case SET_AFS:
		mpu->status = READ_IMU;
	default:
		return;
	}
}

uint8_t mpu9250_read_pid(MPU9250_HandleTypeDef* mpu)
{
	uint8_t response = 0;
	response = mpu9250_write_reg(mpu, MPUREG_WHOAMI|READ_FLAG, 0x00);
	return response;
}

uint8_t mpu9250_read_ak8963_pid(MPU9250_HandleTypeDef* mpu)
{
	uint8_t response;
	/* Set the I2C slave address of AK8963 and set for read. */
	mpu9250_write_reg(mpu, MPUREG_I2C_SLV0_ADDR,AK8963_I2C_ADDR|READ_FLAG);
	mpu9250_write_reg(mpu, MPUREG_I2C_SLV0_REG, AK8963_WIA); /* I2C slave 0 register address from where to begin data transfer */
	mpu9250_write_reg(mpu, MPUREG_I2C_SLV0_CTRL, 0x81); /* Read 1 byte from the magnetometer */
	response = mpu9250_write_reg(mpu, MPUREG_EXT_SENS_DATA_00|READ_FLAG, 0x00);    /* Read I2C */
	return response;
}

uint8_t mpu9250_write_reg(MPU9250_HandleTypeDef* mpu,
			  uint8_t reg_addr,
			  uint8_t reg_data)
{
	uint8_t temp_val = 0;
	uint8_t _reg_addr = reg_addr;
	uint8_t _reg_data = reg_data;
	spi_select(MPU9250_CS_PIN, 1);
	HAL_SPI_Transmit(mpu->spi, &_reg_addr, 1, 1000);
	HAL_SPI_TransmitReceive(mpu->spi, &_reg_data, &temp_val, 1, 1000);
	spi_select(MPU9250_CS_PIN, 0);
	return temp_val;
}

uint8_t mpu9250_read_reg(MPU9250_HandleTypeDef* mpu, uint8_t reg_addr)
{
	return mpu9250_write_reg(mpu, reg_addr | READ_FLAG, 0xff);
}

void mpu9250_read_regs(MPU9250_HandleTypeDef* mpu,
		       uint8_t reg_addr,
		       uint8_t* data,
		       uint8_t len)
{
	uint8_t address = reg_addr | READ_FLAG;
	spi_select(MPU9250_CS_PIN, 1);
	HAL_SPI_Transmit(mpu->spi, &address, 1, 1000);
	HAL_SPI_Receive(mpu->spi, data, len, 1000);
	spi_select(MPU9250_CS_PIN, 0);
}

uint8_t mpu9250_nonblocking_init(MPU9250_HandleTypeDef* mpu)
{
	static uint8_t delay_cnt = 0;

	switch (mpu->initStructure.status) {
	case INIT_RESET:
		if (delay_cnt++ == 0) {
			mpu->rx_byte = mpu_rx_buf;
			mpu9250_write_reg(mpu, MPUREG_PWR_MGMT_1, 0x80);
		} else if (delay_cnt++ > mpu->init_delay) {
			delay_cnt = 0;
			mpu->initStructure.status = INIT_CLK_SEL;
		}
		return INIT_RESET;
	case INIT_CLK_SEL:
		if (delay_cnt++ == 0) {
			mpu9250_write_reg(mpu, MPUREG_PWR_MGMT_1,
					  mpu->initStructure.clk_sel);
		} else if (delay_cnt++ > mpu->init_delay) {
			delay_cnt = 0;
			mpu->initStructure.status = INIT_ENALE_SENSOR;
		}
		return INIT_CLK_SEL;
	case INIT_ENALE_SENSOR:
		if (delay_cnt++ == 0) {
			mpu9250_write_reg(mpu, MPUREG_PWR_MGMT_2, 0x00);
		} else if (delay_cnt++ > mpu->init_delay) {
			delay_cnt = 0;
			mpu->initStructure.status = INIT_GLPF;
		}
		return INIT_ENALE_SENSOR;
	case INIT_GLPF:
		if (delay_cnt++ == 0) {
			mpu9250_write_reg(mpu, MPUREG_CONFIG,
					  mpu->initStructure.gyr_lpf_bw);
		} else if (delay_cnt++ > mpu->init_delay) {
			delay_cnt = 0;
			mpu->initStructure.status = INIT_GFS;
		}
		return INIT_GLPF;
	case INIT_GFS:
		if (delay_cnt++ == 0) {
			mpu9250_write_reg(mpu, MPUREG_GYRO_CONFIG,
					  mpu->initStructure.gyr_fs
					  & MPU9250_GFS_BITMASK);
		} else if (delay_cnt++ > mpu->init_delay) {
			delay_cnt = 0;
			mpu->initStructure.status = INIT_AFS;
		}
		return INIT_GFS;
	case INIT_AFS:
		if (delay_cnt++ == 0) {
			mpu9250_write_reg(mpu, MPUREG_ACCEL_CONFIG,
					  mpu->initStructure.acc_fs
					  & MPU9250_AFS_BITMASK);
		} else if (delay_cnt++ > mpu->init_delay) {
			delay_cnt = 0;
			mpu->initStructure.status = INIT_ALPF;
		}
		return INIT_AFS;
	case INIT_ALPF:
		if (delay_cnt++ == 0) {
			mpu9250_write_reg(mpu, MPUREG_ACCEL_CONFIG_2,
					  mpu->initStructure.acc_lpf_bw);
		} else if (delay_cnt++ > mpu->init_delay) {
			delay_cnt = 0;
			mpu->initStructure.status = INIT_INTCFG;
		}
		return INIT_ALPF;
	case INIT_INTCFG:
		if (delay_cnt++ == 0) {
			mpu9250_write_reg(mpu, MPUREG_INT_PIN_CFG, 0x30);
		} else if (delay_cnt++ > mpu->init_delay) {
			delay_cnt = 0;
			mpu->initStructure.status = INIT_I2C_MASTER;
		}
		return INIT_INTCFG;
	case INIT_I2C_MASTER:
		if (delay_cnt++ == 0) {
			mpu9250_write_reg(mpu, MPUREG_USER_CTRL, 0x20);
		} else if (delay_cnt++ > mpu->init_delay) {
			delay_cnt = 0;
			mpu->initStructure.status = INIT_I2C_RATE;
		}
		return INIT_I2C_MASTER;
	case INIT_I2C_RATE:
		if (delay_cnt++ == 0) {
			mpu9250_write_reg(mpu, MPUREG_I2C_MST_CTRL, 0x0D);
		} else if (delay_cnt++ > mpu->init_delay) {
			delay_cnt = 0;
			mpu->initStructure.status = INIT_MAG_I2C_ADDR;
		}
		return INIT_I2C_RATE;
	case INIT_MAG_I2C_ADDR:
		if (delay_cnt++ == 0) {
			mpu9250_write_reg(mpu, MPUREG_I2C_SLV0_ADDR,
					  AK8963_I2C_ADDR);
		} else if (delay_cnt++ > mpu->init_delay) {
			delay_cnt = 0;
			mpu->initStructure.status = INIT_MAG_RESET_ADDR;
		}
		return INIT_MAG_I2C_ADDR;
	case INIT_MAG_RESET_ADDR:
		if (delay_cnt++ == 0) {
			mpu9250_write_reg(mpu, MPUREG_I2C_SLV0_REG,
					  AK8963_CNTL2);
		} else if (delay_cnt++ > mpu->init_delay) {
			delay_cnt = 0;
			mpu->initStructure.status = INIT_MAG_RESET_DATA;
		}
		return INIT_MAG_RESET_ADDR;
	case INIT_MAG_RESET_DATA:
		if (delay_cnt++ == 0) {
			mpu9250_write_reg(mpu, MPUREG_I2C_SLV0_DO, 0x01);
		} else if (delay_cnt++ > mpu->init_delay) {
			delay_cnt = 0;
			mpu->initStructure.status = INIT_MAG_SEND_RESET;
		}
		return INIT_MAG_RESET_DATA;
	case INIT_MAG_SEND_RESET:
		if (delay_cnt++ == 0) {
			mpu9250_write_reg(mpu, MPUREG_I2C_SLV0_CTRL, 0x81);
		} else if (delay_cnt++ > mpu->init_delay) {
			delay_cnt = 0;
			mpu->initStructure.status = INIT_MAG_CTRL_ADDR;
		}
		return INIT_MAG_SEND_RESET;
	case INIT_MAG_CTRL_ADDR:
		if (delay_cnt++ == 0) {
			mpu9250_write_reg(mpu, MPUREG_I2C_SLV0_REG,
					  AK8963_CNTL1);
		} else if (delay_cnt++ > mpu->init_delay) {
			delay_cnt = 0;
			mpu->initStructure.status = INIT_MAG_CTRL_DATA;
		}
		return INIT_MAG_CTRL_ADDR;
	case INIT_MAG_CTRL_DATA:
		if (delay_cnt++ == 0) {
			mpu9250_write_reg(mpu, MPUREG_I2C_SLV0_DO, 0x12);
		} else if (delay_cnt++ > mpu->init_delay) {
			delay_cnt = 0;
			mpu->initStructure.status = INIT_MAG_SEND_CTRL;
		}
		return INIT_MAG_CTRL_DATA;
	case INIT_MAG_SEND_CTRL:
		if (delay_cnt++ == 0) {
			mpu9250_write_reg(mpu, MPUREG_I2C_SLV0_CTRL, 0x81);
		} else if (delay_cnt++ > mpu->init_delay) {
			delay_cnt = 0;
			mpu->initStructure.status = INIT_COMPLETE;
		}
		return INIT_MAG_SEND_CTRL;
	case INIT_COMPLETE:
		return INIT_COMPLETE;
	default:
		mpu->initStructure.status = INIT_RESET;
		return INIT_RESET;
	}
}

void mpu9250_handle_all_byte(void* pMpu)
{
	int16_t bit_data;
	uint8_t i;
	float acc_sens = 0.0f;
	float gyr_sens = 0.0f;
	MPU9250_HandleTypeDef* mpu = (MPU9250_HandleTypeDef*)pMpu;

	switch (mpu->initStructure.acc_fs) {
	case MPU9250_AFS_2G:
		acc_sens = MPU9250A_2g;
		break;
	case MPU9250_AFS_4G:
		acc_sens = MPU9250A_4g;
		break;
	case MPU9250_AFS_8G:
		acc_sens = MPU9250A_8g;
		break;
	case MPU9250_AFS_16G:
		acc_sens = MPU9250A_16g;
		break;
	default:
		acc_sens = 1.0f;
		break;
	}
	//Get accelerometer value
	for (i=0; i<3; i++) {
		bit_data=((int16_t)mpu->rx_byte[i*2]<<8)|mpu->rx_byte[i*2+1];
		mpu->raw_data.imu.acc[i] = (int16_t)(bit_data * acc_sens);
	}

	//Get temperature
	bit_data = ((int16_t)mpu->rx_byte[i*2]<<8)|mpu->rx_byte[i*2+1];
	mpu->raw_data.temp = (int16_t)(bit_data * MPU9250T_85degC);

	switch (mpu->initStructure.gyr_fs) {
	case MPU9250_GFS_250:
		gyr_sens = MPU9250G_250dps;
		break;
	case MPU9250_GFS_500:
		gyr_sens = MPU9250G_500dps;
		break;
	case MPU9250_GFS_1000:
		gyr_sens = MPU9250G_1000dps;
		break;
	case MPU9250_GFS_2000:
		gyr_sens = MPU9250G_2000dps;
		break;
	default:
		gyr_sens = 1.0f;
		break;
	}
	//Get gyroscop value
	for (i=4; i<7; i++) {
		bit_data=((int16_t)mpu->rx_byte[i*2]<<8)|mpu->rx_byte[i*2+1];
		mpu->raw_data.imu.gyr[i-4] = (int16_t)(bit_data * gyr_sens);
	}

	//Get Magnetometer value
	for(i=7; i<10; i++) {
		bit_data=((int16_t)mpu->rx_byte[i*2+1]<<8)|mpu->rx_byte[i*2];
		mpu->raw_data.mag.mag[i-7] = bit_data;
		//		mpu->raw_data.mag.mag[i-7] = (int16_t)(bit_data * MPU9250M_4800uT);
	}

	event_post(&event_queue, EVENT_IMU_DATA, &mpu->raw_data.imu);
	event_post(&event_queue, EVENT_MAG_DATA, &mpu->raw_data.mag);
}

void mpu9250_handle_imu_byte(void *pMpu)
{
	int16_t bit_data;
	uint8_t i;
	float acc_sens = 0.0f;
	float gyr_sens = 0.0f;
	MPU9250_HandleTypeDef *mpu = (MPU9250_HandleTypeDef *)pMpu;

	switch (mpu->initStructure.acc_fs) {
	case MPU9250_AFS_2G:
		acc_sens = MPU9250A_2g;
		break;
	case MPU9250_AFS_4G:
		acc_sens = MPU9250A_4g;
		break;
	case MPU9250_AFS_8G:
		acc_sens = MPU9250A_8g;
		break;
	case MPU9250_AFS_16G:
		acc_sens = MPU9250A_16g;
		break;
	default:
		acc_sens = 1.0f;
		break;
	}
	//Get accelerometer value
	for (i=0; i<3; i++) {
		bit_data=((int16_t)mpu->rx_byte[i*2]<<8)|mpu->rx_byte[i*2+1];
		mpu->raw_data.imu.acc[i] = (int16_t)(bit_data * acc_sens);
	}

	//Get temperature
	bit_data = ((int16_t)mpu->rx_byte[i*2]<<8)|mpu->rx_byte[i*2+1];
	mpu->raw_data.temp = (int16_t)(bit_data * MPU9250T_85degC);

	switch (mpu->initStructure.gyr_fs) {
	case MPU9250_GFS_250:
		gyr_sens = MPU9250G_250dps;
		break;
	case MPU9250_GFS_500:
		gyr_sens = MPU9250G_500dps;
		break;
	case MPU9250_GFS_1000:
		gyr_sens = MPU9250G_1000dps;
		break;
	case MPU9250_GFS_2000:
		gyr_sens = MPU9250G_2000dps;
		break;
	default:
		gyr_sens = 1.0f;
		break;
	}
	//Get gyroscop value
	for (i=4; i<7; i++) {
		bit_data=((int16_t)mpu->rx_byte[i*2]<<8)|mpu->rx_byte[i*2+1];
		mpu->raw_data.imu.gyr[i-4] = (int16_t)(bit_data * gyr_sens);
	}

	event_post(&event_queue, EVENT_IMU_DATA, &mpu->raw_data.imu);
}
